/*------------------------------------------------------------------------------*/
/* Team 614 2012 Codebase														*/
/*------------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Watchdog;
import edu.wpi.first.wpilibj.Dashboard;
import edu.wpi.first.wpilibj.DigitalModule;
import edu.wpi.first.wpilibj.AnalogModule;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.Servo;
/*
 * the project.
 */
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Victor;
import java.lang.Integer;

public class DefaultRobot extends IterativeRobot
{

	//JOYSTICK
	Joystick m_rightStick;					// joystick 1 (arcade or right tank)
	Joystick m_leftStick;					// joystick 2 (tank left stick)

	// declare joystick variables
	static final int NUM_JOYSTICK_BUTTONS = 16;
	boolean[] m_rightStickButtonState = new boolean[(NUM_JOYSTICK_BUTTONS + 1)];
	boolean[] m_leftStickButtonState = new boolean[(NUM_JOYSTICK_BUTTONS + 1)];

	//ROBOT DRIVE
	RobotDrive m_robotDrive;				// drive object

	//SERVO
	Servo xCam = new Servo(6); 				//to move camera in x-direction
	Servo yCam = new Servo(7); 				// to move camera in y-direction

	//CAMERA
	AxisCamera Cam;							// camera object

	// TIMER for autonomous mode
	Timer shooterTimer;

	//DIO
	DigitalOutput m_DigOut;					// DigOut object
	DriverStation m_driverStation;			// driver station object
	int m_priorPacketNumber;				// store most DS recent pn
	int m_dsPacketsReceivedInCurrentSecond; // current DS packet number

	// SOLENOID
	static final int NUM_SOLENOIDS = 8;
	Solenoid[] m_solenoids = new Solenoid[NUM_SOLENOIDS];

	// drive mode selection
	static final int UNINITIALIZED_DRIVE = 0;
	int m_driveMode;

	// Local variables to count the number of periodic loops performed
	int m_autoPeriodicLoops;
	int m_disabledPeriodicLoops;
	int m_telePeriodicLoops;
	int shooterCount = 0;
	int camCount = 0;
	int debounceCount = 1;
	int allDIO=0, allDIORev=0;
	int dioDir=0, dioDirRev=0;

	//ENCODER
	Encoder encodeDriveL;
	Encoder encodeDriveR;
	Encoder encodeShootL;
	Encoder encodeShootR;
	//Encoder encodeLadder;		// only 4 encoders can be instantiated on robot

	//BOOLEANS
	boolean shooterOn = false;
	boolean ladderState;

	//DOUBLES
	double initialShooterSpeed = 1.0;
	double shLtSpeed;
	double shRtSpeed;
	int this_encShLtRate = 0;
	int this_encShRtRate = 0;
	int last_encShLtRate = 0;
	int last_encShRtRate = 0;
	int encShLtCount = 0;
	int encShRtCount = 0;
	double xMovement = 0.5;
	double yMovement = 0.5;
	double shooterTime = 0;

	//VICTORS
	Victor shooterMotorL;
	Victor shooterMotorR;
	Victor rampLevel;

	//RELAY
	Relay ladderMotor;

	// create Driver station enhanced IO object
	//DriverStationEnhancedIO dseio;
	byte dseio_mode, last_dseio_mode = 99;

	//DriverStationEnhancedIO.tDigitalConfig[] dseio_DigConfig;

	/*
	 * Constructor for this "BuiltinDefaultCode" Class.
	 *
	 * The constructor creates all of the objects used for the different inputs
	 * and outputs of the robot. Essentially, the constructor defines the
	 * input/output mapping for the robot, providing named objects for each of
	 * the robot interfaces.
	 */
	public DefaultRobot()
	{
		System.out.println("BuiltinDefaultCode Constructor Started\n");

		// Create a robot using standard right/left robot drive on
		// PWMS 1, 2, 3, and #4
		m_robotDrive = new RobotDrive(2, 1);
		m_robotDrive.setSafetyEnabled(false);

		// Acquire the Driver Station object
		m_driverStation = DriverStation.getInstance();
		m_priorPacketNumber = 0;
		m_dsPacketsReceivedInCurrentSecond = 0;

		// Define joysticks being used at USB port #1 and USB port #2 on the DS
		m_rightStick = new Joystick(1);
		m_leftStick = new Joystick(2);

		//INIT RELAY
		ladderMotor = new Relay(1);

		//To Toggle Buttons
		ladderState = false;

		// INIT MOTOR
		shooterMotorL = new Victor(3);
		shooterMotorR = new Victor(4);
		rampLevel = new Victor(5);

		//Create encoder objects
		encodeShootL = new Encoder(7, 8);
		encodeShootL.start();
		encodeShootR = new Encoder(9, 10);
		encodeShootR.start();
		encodeDriveL = new Encoder(3, 4);
		encodeDriveL.start();
		encodeDriveR = new Encoder(5, 6);
		encodeDriveR.start();
		//encodeLadder = new Encoder(11, 12);   // only 4 encoders possible in cRIO
		//encodeLadder.start();

		// Iterate over all the buttons on each joystick, each state set to false
		int buttonNum = 1;	  // start counting buttons at button 1
		for (buttonNum = 1; buttonNum <= NUM_JOYSTICK_BUTTONS; buttonNum++)
		{
			m_rightStickButtonState[buttonNum] = false;
			m_leftStickButtonState[buttonNum] = false;
		}
		// Iterate over all the solenoids on the robot, constructing each in turn
		int solenoidNum = 1;		// start counting solenoids at solenoid 1
		for (solenoidNum = 0; solenoidNum < NUM_SOLENOIDS; solenoidNum++)
		{
			m_solenoids[solenoidNum] = new Solenoid(solenoidNum + 1);
		}

		// Set drive mode to uninitialized
		m_driveMode = UNINITIALIZED_DRIVE;

		// Initialize counters to record the number of loops completed in
		//   autonomous and teleop modes
		m_autoPeriodicLoops = 0;
		m_disabledPeriodicLoops = 0;
		m_telePeriodicLoops = 0;

		System.out.println("BuiltinDefaultCode Constructor Completed\n");
	}

	/**
	 * ******************************** Init Routines ************************
	 */
	public void robotInit()
	{
		// Actions which would be performed once (and only once) upon
		// initialization of the robot would be put here.

		//INIT CAM
		Cam = AxisCamera.getInstance("10.6.14.11");
		Cam.writeResolution(AxisCamera.ResolutionT.k320x240);
		Cam.writeBrightness(50);
		Cam.writeColorLevel(50);
		Cam.writeMaxFPS(60);
		Cam.writeCompression(20);
		Cam.writeRotation(AxisCamera.RotationT.k0);		// picture right side up
		//Cam.writeRotation(AxisCamera.RotationT.k180);	// picture upside down

		// init shooterTimer
		shooterTimer = new Timer();

		System.out.println("RobotInit() completed.\n");
	}

	public void disabledInit()
	{
		m_disabledPeriodicLoops = 0; // Reset the loop counter for disabled mode
		ClearSolenoidLEDsKITT();
		// Move the cursor down a few, since we'll move it back up in periodic.
	}

	public void autonomousInit()
	{
		m_autoPeriodicLoops = 0;  // Reset the loop counter for autonomous mode
		ClearSolenoidLEDsKITT();
		encodeShootL.reset();
		encodeShootR.reset();
		encodeDriveL.reset();
		encodeDriveR.reset();

		// start shooter timer
		shooterTimer.start();

		// turn on shooter
		System.out.println("Autonomous mode - Shooter On.");
		shRtSpeed = -initialShooterSpeed * .913;
		shLtSpeed = initialShooterSpeed;
		shooterMotorR.set(shRtSpeed);
		shooterMotorL.set(shLtSpeed);
		shooterOn = true;
		shooterCount = 0;

		// now turn on ball feed ladder
		System.out.println("Autonomous mode - ladder On");
		ladderMotor.set(Relay.Value.kForward);
		ladderState = true;

	}

	public void teleopInit()
	{
		m_telePeriodicLoops = 0;				// Reset teleop loop counter
		m_dsPacketsReceivedInCurrentSecond = 0; // Reset num dsPackets current sec
		m_driveMode = UNINITIALIZED_DRIVE;		// Set drive mode to uninitialized
		ClearSolenoidLEDsKITT();

		updateDefaultDashboard();
	}
	/**
	 * ******************************** Periodic Routines *********************
	 */
	static int printSec = (int) ((Timer.getUsClock() / 1000000.0) + 1.0);
	static final int startSec = (int) (Timer.getUsClock() / 1000000.0);

	public void disabledPeriodic()
	{
		// feed the user watchdog at every period when disabled
		Watchdog.getInstance().feed();

		// increment the number of disabled periodic loops completed
		m_disabledPeriodicLoops++;

		// while disabled, printout the duration of current disabled mode in sec
		if ((Timer.getUsClock() / 1000000.0) > printSec)
		{
			// Move the cursor back to the previous line and clear it.
			System.out.println("Disabled seconds: " + (printSec - startSec) + "\r\n");
			printSec++;
		}
	}

	public void autonomousPeriodic()
	{
		// feed the user watchdog at every period when in autonomous
		Watchdog.getInstance().feed();

		// increment the number of autonomous periodic loops completed
		m_autoPeriodicLoops++;
		updateDefaultDashboard();

		shooterCount++;
		shooterTime = shooterTimer.get();

		// Stop shooter and ladder after X microsec
		if (shooterOn & shooterTime > 13)
		{
			System.out.println("ShooterTime=" + shooterTime);

			//Stop the shooter Motors
			//System.out.println("Autonomous mode - Shooter Off");
			shooterMotorR.set(0.0);
			shooterMotorL.set(0.0);
			shooterOn = false;

			// Stop the ladder
			//System.out.println("Autonomous mode - ladder Off");
			ladderMotor.set(Relay.Value.kOff);
			ladderState = false;

			//shooterTimer.stop();
			shooterCount = 0;
		}
	}

	public void teleopPeriodic()
	{
		// feed the user watchdog at every period when in autonomous
		Watchdog.getInstance().feed();

		// increment the number of teleop periodic loops completed
		m_telePeriodicLoops++;

		/*
		 * Code placed in here will be called only when a new packet of
		 * information has been received by the Driver Station. Any code which
		 * needs new information from the DS should go in here
		 */

		m_dsPacketsReceivedInCurrentSecond++;   // increment DS packets received

		// put Driver Station-dependent code here

		Solenoid[] firstGroup = new Solenoid[4];
		Solenoid[] secondGroup = new Solenoid[4];

		for (int i = 0; i < 4; i++)
		{
			firstGroup[i] = m_solenoids[i];
			secondGroup[i] = m_solenoids[i + 4];
		}

		JoystickButtons(m_rightStick, m_rightStickButtonState, "Right Stick ", firstGroup);
		JoystickButtons(m_leftStick, m_leftStickButtonState, "Left Stick ", secondGroup);

		// print out motor set speeds and encoder rates
		if (shooterOn == true)
		{
			shooterCount++;
			if (shooterCount > 40)
			{
				shooterCount = 0;   // counter to get rate & print every X loops
				this_encShLtRate = encodeShootL.get();
				this_encShRtRate = encodeShootR.get();
				encShLtCount = this_encShLtRate - last_encShLtRate;
				encShRtCount = this_encShRtRate - last_encShRtRate;
				System.out.println("L-Set/R-Set: " + (int) (1000 * shLtSpeed)
						+ "/" + (int) (1000 * shRtSpeed) + "  L-Enc/R-Enc: "
						+ encShLtCount + "/" + encShRtCount);
				last_encShLtRate = this_encShLtRate;
				last_encShRtRate = this_encShRtRate;
			}
		}

		// use tank drive
		//m_robotDrive.tankDrive(m_leftStick, m_rightStick);

		// use arcade drive
		m_robotDrive.arcadeDrive(m_leftStick);
		rampLevel.set(m_rightStick.getY());	 //get rightStick Y value for rampLevel

		updateDefaultDashboard();
	}

	public void ClearSolenoidLEDsKITT()
	{
		// Iterate over all the solenoids on the robot, clearing each in turn
		int solenoidNum = 1;	// start counting solenoids at solenoid 1
		for (solenoidNum = 0; solenoidNum < NUM_SOLENOIDS; solenoidNum++)
		{
			m_solenoids[solenoidNum].set(false);
		}
	}

	int GetLoopsPerSec()
	{
		return 10000; // calls itself every 1 seconds! used to be 10,000
	}

	/*
	 * Demonstrate handling of joystick buttons
	 *
	 * This method expects to be called during each periodic loop, providing
	 * the following capabilities:
	 * - Print out a message when a button is initially pressed
	 * - Solenoid LEDs light up according to joystick buttons:
	 *   - When no buttons pressed, clear the solenoid LEDs
	 *   - When only one button is pressed, show the button number (in binary)
	 *	 via the solenoid LEDs
	 *   - When more than one button is pressed, show "15" (in binary) via the
	 *	 solenoid LEDs
	 */
	public void JoystickButtons(Joystick currStick, boolean[] buttonPreviouslyPressed,
			String stickString, Solenoid solenoids[])
	{

		int buttonNum = 1;				  // start counting buttons at button 1
		boolean outputGenerated = false;	// flag for whether or not output
		// is generated for a button
		int numOfButtonPressed = 0;		 // 0 if no buttons pressed,
		//-1 if multiple buttons pressed

		/*
		 * Iterate over all the buttons on the joystick, checking to see if each
		 * is pressed. If a button is pressed, check to see if it is newly
		 * pressed; if so, print out a message on the console
		 */
		for (buttonNum = 1; buttonNum <= NUM_JOYSTICK_BUTTONS; buttonNum++)
		{
			if (currStick.getRawButton(buttonNum))
			{
				// the current button is pressed, now act accordingly...
				if (!buttonPreviouslyPressed[buttonNum])
				{

					//LEFT STICK

					//Camera Movement Buttons : 2,3 (down/up Y) & 4,5(lft/rht X)
					// Move Camera in Y-Direction

					if (m_leftStick.getRawButton(2))
					{
						camCount++;
						if(camCount>debounceCount)		// debounce button
						{
							yMovement += 0.025;
							if (yMovement < 0.0)
								yMovement = 0.0;
							if (yMovement > 1.0)
								yMovement = 1.0;

							// round output by parsing to int

							System.out.println("cam y inc, servo="
									+ (int)(1000*yMovement));
							yCam.set(yMovement);
							camCount=0;
						}
					}

					//Move Camera in Y-Direction (opposite)
					if (m_leftStick.getRawButton(3))
					{
						camCount++;
						if(camCount>debounceCount)		// debounce button
						{
							yMovement -= 0.025;
							if (yMovement < 0.0)
								yMovement = 0.0;

							if (yMovement > 1.0)
								yMovement = 1.0;

							// round output by parsing to int
							System.out.println("cam y dec, servo="
									+ (int)(1000*yMovement));
							yCam.set(yMovement);
							camCount=0;
						}
					}

					//Move Camera in X-Direction
					if (m_leftStick.getRawButton(4))
					{
						camCount++;
						if(camCount>debounceCount)		// debounce button
						{
							xMovement -= 0.025;
							if (xMovement < 0.0)
								xMovement = 0.0;

							if (xMovement > 1.0)
								xMovement = 1.0;

							// round output by parsing to int
							System.out.println("cam x dec, servo="
									+ (int)(1000*xMovement));
							xCam.set(xMovement);
							camCount=0;
						}
					}

					//Move Camera in X-Direction (Opposite)
					if (m_leftStick.getRawButton(5))
					{
						camCount++;
						if(camCount>debounceCount)		// debounce button
						{
							xMovement += 0.025;
							if (xMovement < 0.0)
								xMovement = 0.0;

							if (xMovement > 1.0)
								xMovement = 1.0;

							// round output by parsing to int
							System.out.println("cam x inc, servo="
									+ (int)(1000*xMovement));
							xCam.set(xMovement);
							camCount=0;
						}
					}

					if (m_leftStick.getRawButton(10))
					{
						System.out.println("leftStick button 10, Left Shooter speed UP");
						if (shLtSpeed < 1.0)
						{
							shLtSpeed = shLtSpeed + 0.01;
							shooterMotorL.set(shLtSpeed);
						}
						if (shLtSpeed > 1.0)
						{
							shLtSpeed = 1.0;
						}
						System.out.println("Left motor speed: " + shooterMotorR.get() +
								", Left motor speed : " + shooterMotorL.get());
					}

					if (m_leftStick.getRawButton(11))
					{
						System.out.println("Left Stick button 11, Left Shooter speed DOWN");
						if (shLtSpeed > 0.05)
						{
							shLtSpeed = shLtSpeed - 0.01;
							shooterMotorL.set(shLtSpeed);
						}
						if (shLtSpeed < 0.05)
						{
							shLtSpeed = 0.05;
						}
						System.out.println("Left motor speed: " + shooterMotorR.get() +
								", Left motor speed : " + shooterMotorL.get());
					}

					//RIGHT STICK
					if (m_rightStick.getRawButton(1)) // activate shooter
					{
						//run the shooter motors forwards
						System.out.println("rightStick button 1, Shooter On");
						shRtSpeed = -initialShooterSpeed ;
						shRtSpeed = -initialShooterSpeed;
						//experimentally determined, to make speeds equal
						// 91.3% * right shooter = left shooter
						//shLtSpeed = initialShooterSpeed;
						shooterMotorR.set(shRtSpeed);
						shooterMotorL.set(shLtSpeed);
						shooterOn = true;
						shooterCount = 0;
					}

					if (m_rightStick.getRawButton(3))
					{
						//Stop the shooter Motors
						System.out.println("rightStick button 3, Shooter Off");
						shooterMotorR.set(0.0);
						shooterMotorL.set(0.0);
						shooterOn = false;
					}

					//TOGGLE LADDER MOTOR
					if (m_rightStick.getRawButton(6))
					{
						System.out.println("rightStick button 6, ladder On");
						ladderMotor.set(Relay.Value.kForward);
						ladderState = true;
					}

					if (m_rightStick.getRawButton(7))
					{
						System.out.println("rightStick button 7, ladder Off");
						ladderMotor.set(Relay.Value.kOff);
						ladderState = false;
					}

					if (m_rightStick.getRawButton(10))
					{
						System.out.println("rightStick button 10, Right Shooter speed UP");
						if (shRtSpeed > -1.0)
						{
							shRtSpeed = shRtSpeed - 0.01;
							shooterMotorR.set(shRtSpeed);
						}
						if (shRtSpeed < -1.0)
						{
							shRtSpeed = -1.0;
						}
					}

					if (m_rightStick.getRawButton(11))
					{
						System.out.println("rightStick button 11, Right Shooter speed DOWN");
						if (shRtSpeed < -0.05)
						{
							shRtSpeed = shRtSpeed + 0.01;
							shooterMotorR.set(shRtSpeed);
						}
						if (shRtSpeed > -0.05)
						{
							shRtSpeed = -0.05;
						}
						System.out.println("Right motor speed: " + shooterMotorR.get() +
								", Left motor speed : " + shooterMotorL.get());
					}

					// remember that this button is pressed for the next iteration
					buttonPreviouslyPressed[buttonNum] = true;

					// set numOfButtonPressed appropriately
					if (numOfButtonPressed == 0)
					{
						// no button pressed yet this time through, set the num correctly
						numOfButtonPressed = buttonNum;
					} else
					{						// another button (or buttons) must have already been
						// pressed, set appropriately
						numOfButtonPressed = -1;
					}
				} else
				{
					buttonPreviouslyPressed[buttonNum] = false;
				}

				if (numOfButtonPressed == -1)
				{
					// multiple buttons were pressed, display as if button 15 was pressed
					DisplayBinaryNumberOnSolenoidLEDs(15, solenoids);
				} else
				{
					// display the number of the button pressed on the solenoids;
					// note that if no button was pressed (0), the solenoid display
					// will be cleared (set to 0)
					DisplayBinaryNumberOnSolenoidLEDs(numOfButtonPressed, solenoids);
				}
			}
		}
	}

	/*
	 * Display a given four-bit value in binary on the given solenoid LEDs
	 */
	void DisplayBinaryNumberOnSolenoidLEDs(int displayNumber, Solenoid[] solenoids)
	{

		if (displayNumber > 15)
		{
			// if the number to display is larger than can be displayed in
			// 4 LEDs, display 0 instead
			displayNumber = 0;
		}

		solenoids[3].set((displayNumber & 1) != 0);
		solenoids[2].set((displayNumber & 2) != 0);
		solenoids[1].set((displayNumber & 4) != 0);
		solenoids[0].set((displayNumber & 8) != 0);
	}

	public void updateDefaultDashboard()
	{
		Dashboard lowDashData = DriverStation.getInstance().getDashboardPackerLow();
		lowDashData.addCluster();
		{
			lowDashData.addCluster();
			{	//analog modules
				lowDashData.addCluster();
				{
					for (int i = 1; i <= 8; i++)
					{
						lowDashData.addFloat(
								(float) AnalogModule.getInstance(1).getAverageVoltage(i));
					}
				}
				lowDashData.finalizeCluster();

				// send dummy 'analog module 2' values, use this section in only 1 analog
				lowDashData.addCluster();
				{
					for (int i = 1; i <= 8; i++)
					{
						lowDashData.addFloat(1);
					}
				}
				lowDashData.finalizeCluster();
			}
			lowDashData.finalizeCluster();

			lowDashData.addCluster();
			{ //digital modules
				lowDashData.addCluster();
				{
					lowDashData.addCluster();
					{
						int module = 1;
						lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
						lowDashData.addByte(DigitalModule.getInstance(module).getRelayReverse());
						allDIO = DigitalModule.getInstance(module).getAllDIO();
						allDIORev = bitReversal(allDIO);
						lowDashData.addShort((short)allDIORev);		//reverse bit order
						//lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
						dioDir = DigitalModule.getInstance(module).getDIODirection();
						dioDirRev = bitReversal(dioDir);
						lowDashData.addShort((short)dioDirRev);		//reverse bit order
						//lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
						lowDashData.addCluster();
						{
							for (int i = 1; i <= 10; i++)
							{
								lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
							}
						}
						lowDashData.finalizeCluster();
					}
					lowDashData.finalizeCluster();
				}
				lowDashData.finalizeCluster();

				lowDashData.addCluster();
				{
					// dummy values for digital module 2
					lowDashData.addCluster();
					{
						lowDashData.addByte((byte) 1);
						lowDashData.addByte((byte) 1);
						lowDashData.addShort((short) 1);
						lowDashData.addShort((short) 1);
						lowDashData.addCluster();
						{
							for (int i = 1; i <= 10; i++)
							{
								lowDashData.addByte((byte) 1);
							}
						}
						lowDashData.finalizeCluster();
					}
					lowDashData.finalizeCluster();
				}
				lowDashData.finalizeCluster();
			}
			lowDashData.finalizeCluster();
			lowDashData.addByte(m_solenoids[1].getAll());
		}
		lowDashData.finalizeCluster();
		lowDashData.commit();
	}
	public int bitReversal(int x)
	{
		// NOTE: FRC Driver Station developed and written in NI LabView
		//       LabView uses 'C' convention 'little' endian
		//       Java 'short' uses 'big' endian
		//       -->DS bit displays for GPIOState / GPIOOutputEnable are backwards!
		// This method 'reverses' bit order
		// i.e.   0000 0001 0110 1011  --> 1101 0110 1000 0000
		// Loop logic:
		//    input variable is x, output variable is y (and initialized to zero)
		//    loop is iterated X times
		//    1. y is left bit shifted (<<=)
		//    2. the least significant bit (LSB) of x is determined  (x&1)
		//       the LSB of x is then added to y
		//    3. x is right bit shifted (>>=)
		// end of loop, repeat X times
		// loop:
		//     y is promoted (left bit shifted)
		//     the LSB of x is bitmasked and stuffed into y
		//     x is demoted (right bit shifted) i.e. LSB is 'peeled' off and discarded
		//     loop repeated
		int y=0;
		for (int i = 0; i<16; ++i)
		{  //use bitwise operators to reverse bit order
			y <<= 1;			// left bit shift y
			y |= (x & 1);		// then add LSB of x to y
			x >>= 1;			// right bit shift x
		}
		//System.out.println("bit reversal y=" + y);
		return y;
	}
	public String printShort(short num)
	{
		// this method accepts an short and returns a string formated in hex and bin
		int zero_byte, first_byte, second_byte, third_byte;
		String out;
		zero_byte = num & 0x0f;				// bitwise AND to get 0th byte
		first_byte = (num & 0xf0) >> 4;     // bitwise AND, then bitshift to get 1st byte
		second_byte = (num & 0x0f00) >> 8;  // bitwise AND, then bitshift to get 2nd byte
		third_byte = (num & 0xf000) >> 12;  // bitwise AND, then bitshift to get 3rd byte
		out = "0x " + byteToHexStr(zero_byte) +
				byteToHexStr(first_byte) + byteToHexStr(second_byte) +
				byteToHexStr(third_byte);
		out = out + "    0b " + byteToBinStr(zero_byte) + " "
				+ byteToBinStr(first_byte) + " " + byteToBinStr(second_byte) + " "
				+ byteToBinStr(third_byte);
		return out;
	}
	public String printHex(int num)
	{
		// this method accepts an int and returns a string formated in hex and bin
		int lobyte, hibyte;
		String out;
		lobyte = num & 0x0f;			// bitwise AND to get low byte
		hibyte = (num & 0xf0) >> 4;     // bitwise AND, then bitshift to get hi byte
		out = "0x " + byteToHexStr(hibyte) + byteToHexStr(lobyte);
		out = out + "     0b " + byteToBinStr(hibyte) + " " + byteToBinStr(lobyte);
		return out;
	}
	public String byteToHexStr( int input)
	{
		// this method accepts an input int and returns a string formated in hex
		String out;
		switch (input)
		{
			case 0: out = "0";
				break;
			case 1: out = "1";
				break;
			case 2: out = "2";
				break;
			case 3: out = "3";
				break;
			case 4: out = "4";
				break;
			case 5: out = "5";
				break;
			case 6: out = "6";
				break;
			case 7: out = "7";
				break;
			case 8: out = "8";
				break;
			case 9: out = "9";
				break;
			case 10: out = "a";
				break;
			case 11: out = "b";
				break;
			case 12: out = "c";
				break;
			case 13: out = "d";
				break;
			case 14: out = "e";
				break;
			case 15: out = "f";
				break;
			default: out = "x"; // return 'x' if no match
				break;
		}
		return out;
	}
	public String byteToBinStr( int input)
	{
		// this method accepts an input int and returns a string formated in bin
		String out;
		switch (input)
		{
			case 0: out = "0000";
				break;
			case 1: out = "0001";
				break;
			case 2: out = "0010";
				break;
			case 3: out = "0011";
				break;
			case 4: out = "0100";
				break;
			case 5: out = "0101";
				break;
			case 6: out = "0110";
				break;
			case 7: out = "0111";
				break;
			case 8: out = "1000";
				break;
			case 9: out = "1001";
				break;
			case 10: out = "1010";
				break;
			case 11: out = "1011";
				break;
			case 12: out = "1100";
				break;
			case 13: out = "1101";
				break;
			case 14: out = "1110";
				break;
			case 15: out = "1111";
				break;
			default: out = "x"; // return 'x' if no match
				break;
		}
		return out;
	}
}
